///////////////////////////////////////////////////////////////////////////////////////////////////
/// \file formic/sr1.h
///
/// \brief   header file for the SR1 method
///
///////////////////////////////////////////////////////////////////////////////////////////////////

#ifndef FORMIC_SR1_HEADER
#define FORMIC_SR1_HEADER

#include<vector>
#include<numeric>
#include<cassert>
#include<algorithm>
#include<cmath>

#include<boost/math/special_functions/fpclassify.hpp>

#include<formic/mpi/interface.h>
#include<formic/output/output.h>
#include<formic/exception.h>
#include<formic/lapack/interface.h>
#include<formic/numeric/numeric.h>
#include<formic/lbfgs.h>

namespace formic {

  ///////////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief  Performs a quasi-Newton minimization of the target function with the
  ///         Hessian approximated by the symmetric rank 1 (SR1) update procedure 
  ///
  /// \param[in,out]  userinp        object used to control the optimization
  /// \param[in,out]  x0             the initial guess.  upon exit, x0 is destroyed
  /// \param[out]     x1             upon exit, the lowest-value point found
  ///
  ///////////////////////////////////////////////////////////////////////////////////////////////////
  template <class W>
  inline void simple_sr1(W & worker,
                         const int max_iter,
                         double grad_thresh,
                         formic::Vector<double,int> & x0,
                         formic::Vector<double,int> & x1,
                         const bool print = true,
                         const bool grad_by_ratio = false) {

    // get MPI info
    const int nproc = formic::mpi::size();
    const int myrank = formic::mpi::rank();

    // get the number of variables
    const int n = x0.size();

    // initialize the hessian approximation
    formic::Matrix<double,int> hess = formic::identity_matrix<double,int>(n);

    // prepare variables for the target function values
    double f0, f1;

    // get space to hold gradients
    formic::Vector<double,int> g_old(n, 0.0);
    formic::Vector<double,int> g_new(n, 0.0);

    // compute the initial target function value
    worker.get_value(n, &x0.at(0), f0);

    // compute the initial gradient and place it 
    worker.get_gradient(n, &x0.at(0), &g_old.at(0));

    // print position and gradient
    worker.print(n, "initial position", &x0.at(0));
    worker.print(n, "initial gradient", &g_old.at(0));

    // compute and print the norm of the initial gradient
    if (myrank == 0) {
      const double grad_norm = std::abs( std::sqrt( formic::xdotc(n, &g_old.at(0), 1, &g_old.at(0), 1) ) );
      if (print) {
        formic::of << boost::format(" function val = %20.12f") % f0 << std::endl;
        formic::of << boost::format("gradient norm = %20.12f") % grad_norm << std::endl << std::endl;
      }
      if (grad_by_ratio)
        grad_thresh *=  grad_norm;
    }

    // set the initial step size to try
    double step_size = 1.0;

    // iterate
    bool converged = false;
    int m_used = 0;
    for (int iter = 0; iter < max_iter; iter++) {

      // print a greeting
      if (myrank == 0 && print)
        formic::of << boost::format("Simple SR1:  starting iteration %4i") % iter << std::endl << std::endl;

      // compute the search direction on the root process and store it in g_new
      if (myrank == 0) {
        formic::Vector<int,int> ipiv(n, 0);
        g_new <<= g_old;
        int info = 0;
        formic::dgesv(n, 1, &hess.clone().at(0,0), n, &ipiv.at(0), &g_new.at(0), n, info);
        if ( info != 0 )
          throw formic::Exception("formic::dgesv failed with info = %i in formic::simple_sr1") % info;
        g_new *= -1.0;
      }

      // print the search direction
      worker.print(n, "search direction", &g_new.at(0));

      // perform a line search to find the updated position x1
      step_size = 1.0;
      formic::lbfgs_loose_line_search(worker, n, &x0.at(0), &g_new.at(0), &x1.at(0), step_size);

      // if the step size was zero, we cannot descend
      formic::mpi::bcast(step_size);
      //if ( myrank == 0 && print && step_size == 0.0 )
      //  formic::of << boost::format("computed search direction failed to lower the target function value, stopping simple sr1...") << std::endl << std::endl;
      //if ( step_size == 0.0 )
      //  break;

      // compute new value
      worker.get_value(n, &x1.at(0), f1);

      // compute new gradient
      worker.get_gradient(n, &x1.at(0), &g_new.at(0));

      // print the new position and gradient
      worker.print(n, "new position", &x1.at(0));
      worker.print(n, "new gradient", &g_new.at(0));

      // compute and print the norm of the new gradient
      const double grad_norm = ( myrank == 0 ? std::abs( std::sqrt( formic::xdotc(n, &g_new.at(0), 1, &g_new.at(0), 1) ) ) : 0.0 );
      if (myrank == 0 && print) {
        formic::of << boost::format(" function val = %20.12f") % f1 << std::endl;
        formic::of << boost::format("gradient norm = %20.12f") % grad_norm << std::endl << std::endl;
      }

      // if the gradient is small enough, stop iterating
      converged = (grad_norm < grad_thresh);
      formic::mpi::bcast(converged);
      if (converged) break;

      // update the hessian
      if (myrank == 0) {

        // get gradient difference
        g_old = g_new - g_old;

        // get position difference
        x0 = x1 - x0;

        // compute the "gradient residual"
        formic::Vector<double,int> g_res = g_old - hess * x0;

        // update the approximate hessian
        hess += g_res.m() * g_res.t() / ( g_res.t() * x0 ).at(0);

      }

      // perform any tasks requested to be done when a new position is found
      worker.process_new_point(n, &x1.at(0));

      // compute new value
      worker.get_value(n, &x1.at(0), f1);

      // compute new gradient
      worker.get_gradient(n, &x1.at(0), &g_new.at(0));

      // move the new gradient and position into the old gradient and position
      if (myrank == 0) {
        x0 <<= x1;
        g_old <<= g_new;
      }

    }

    // check that the method converged
    if (myrank == 0 && print) {
      if (converged)
        formic::of << boost::format("Simple SR1 method converged!") << std::endl << std::endl;
      else
        formic::of << boost::format("Simple SR1 method did not converge...") << std::endl << std::endl;
    }

  }

}

#endif
